//
// Created by host on 8/10/25.
//

#ifndef ZORAL_INTERFACE_H
#define ZORAL_INTERFACE_H

#include <memory>
#include "model/differential_kinematics.hpp"
#include "model/forward_kinematics.hpp"
#include "model/inverse_dynamics.hpp"
#include "model/inverse_kinematics.hpp"
#include "plan/robot_plan.h"

namespace ZORAL
{
    class ZoralInterface
    {
    public:
        ZoralInterface(const std::string &robot_name, const std::string &config_path)
        {
            rm_ = std::make_shared<RobotModel>(robot_name, config_path);
            fk_ = std::make_shared<ForwardKinematics>(rm_);
            dk_ = std::make_shared<DifferentialKinematics>(fk_);
            ik_ = std::make_shared<InverseKinematics>(fk_);
            id_ = std::make_shared<InverseDynamics>(fk_);
            rp_ = std::make_shared<RobotPlan>(dk_, ik_);
            dof_ = rm_->getRobotDOF();
        }

    // private:
        std::shared_ptr<RobotModel> rm_;
        std::shared_ptr<ForwardKinematics> fk_;
        std::shared_ptr<DifferentialKinematics> dk_;
        std::shared_ptr<InverseKinematics> ik_;
        std::shared_ptr<InverseDynamics > id_;

        std::shared_ptr<RobotPlan> rp_;

        int dof_{};
    };
} // namespace ZORAL


#endif // ZORAL_INTERFACE_H
